
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       p.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "p.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Proportional controller with piecewise sqrt sections to constrain second derivative
float p_ctrl_sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
    float correction_rate;
    if (math_flt_negative(second_ord_lim) || math_flt_zero(second_ord_lim)) {
        // second order limit is zero or negative.
        correction_rate = error * p;
    } else if (math_flt_zero(p)) {
        // P term is zero but we have a second order limit.
        if (math_flt_positive(error)) {
            correction_rate = math_sqrtf(2.0f * second_ord_lim * (error));
        } else if (math_flt_negative(error)) {
            correction_rate = -math_sqrtf(2.0f * second_ord_lim * (-error));
        } else {
            correction_rate = 0.0f;
        }
    } else {
        // Both the P and second order limit have been defined.
        float linear_dist = second_ord_lim / (p*p);
        if (error > linear_dist) {
            correction_rate = math_sqrtf(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
        } else if (error < -linear_dist) {
            correction_rate = -math_sqrtf(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)));
        } else {
            correction_rate = error * p;
        }
    }
    if (!math_flt_zero(dt)) {
        // this ensures we do not get small oscillations by over shooting the error correction in the last time step.
        return math_constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
    } else {
        return correction_rate;
    }
}

// inverse of the sqrt controller.  calculates the input (aka error) to the sqrt_controller required to achieve a given output
float inv_sqrt_controller(float output, float p, float D_max)
{
    if (math_flt_positive(D_max) && math_flt_zero(p)) {
        return (output * output) / (2.0 * D_max);
    }
    if ((math_flt_negative(D_max) || math_flt_zero(D_max)) && !math_flt_zero(p)) {
        return output / p;
    }
    if ((math_flt_negative(D_max) || math_flt_zero(D_max)) && math_flt_zero(p)) {
        return 0.0;
    }

    // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
    const float linear_velocity = D_max / p;

    if (fabsf(output) < linear_velocity) {
        // if our current velocity is below the cross-over point we use a linear function
        return output / p;
    }

    const float linear_dist = D_max / sq(p);
    const float stopping_dist = (linear_dist * 0.5f) + sq(output) / (2.0 * D_max);
    return math_flt_positive(output) ? stopping_dist : -stopping_dist;
}
/*------------------------------------test------------------------------------*/


